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DETERMINATION  OF  AN  OPTIMAL  CONTROL  STRATEGY 
FOR  A  GENERIC  SURFACE  VEHICLE 


INTRODUCTION 


A  critical  component  of  all  autonomous  mechanical  systems  is  the  ability  to  follow 
prescribed  motion  trajectories.  In  particular,  for  autonomous  vehicles,  this  motion  trajectory  is 
given  by  the  determination  of  the  motion  path  for  the  vehicle  to  follow  to  get  from  one  point  to 
another.  Determining  the  best  path  to  take  and  performing  the  mechanical  functions  to  follow 
that  path  (that  is,  setting  a  thrust  and  steering)  are  basic  functions  of  all  driving  and  piloting  that 
are  often  taken  for  granted  when  the  vehicle  is  a  manned  (or  remotely  operated)  system.  For 
autonomous  military  systems,  however,  optimal  path  planning  and  optimal  vehicle  control  are 
not  simple,  routine  functions;  they  are  complex  operations  affected  by  many  parameters.  As 
more  and  more  tasks  are  transferred  to  autonomous  agents,  determining  an  optimal  control 
strategy  becomes  increasingly  crucial  to  system  performance  to  assure  platform  safety,  platform 
efficiency,  and  successful  mission  completion. 

This  report  documents  the  research  that  was  conducted  to  determine  an  optimal  control 
strategy  for  a  generic  surface  vehicle.  Specifically,  this  report  focuses  on  planning  the  optimal 
path  for  a  generic  surface  vehicle  (boat)  that  is  operating  in  a  region  with  non-negligible  surface 
currents.  Given  a  known  initial  position,  the  planning  goal  for  this  investigation  was  to  find  a 
path  to  get  to  a  prescribed  ending  position  at  a  prescribed  time,  while  expending  a  minimal 
amount  of  energy.  The  steps  necessary  to  achieve  that  goal,  described  in  the  ensuing  sections, 
include  such  topics  as  (1)  a  mathematical  description  of  a  dynamic  model  of  the  surface  vehicle, 
(2)  a  numerical  investigation  showing  the  qualitative  validity  of  the  model,  (3)  a  two-point 
boundary  value  problem  formulation  of  the  path  planning  goal.  (4)  the  optimization  performed  to 
determine  those  optimal  paths,  (5)  techniques  of  optimal  control  theory  to  achieve  the  optimal 
path,  even  in  situations  when  the  initial  condition  is  varied,  (6)  use  of  optimal  control  methods  to 
provide  a  path  that  is  close  to  the  optimal  trajectory,  even  when  the  available  thrust  for  the 
vehicle  is  limited. 

In  short,  this  report  demonstrates  a  method  for  determining  the  optimal  trajectory  and 
then  using  optimal  control  to  best  follow  that  trajectory  in  practical  situations.  All  of  the  results 
presented  in  this  report  are  simulation  results  run  in  MATLAB  on  a  generic  model  of  surface 
vehicles  operating  in  currents. 
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PLANAR  SURFACE  VEHICLE  MODEL 


This  study  used  a  generic  three  degree-of- freedom  surface  vehicle  model  (figure  1)  that 
considers  surge,  sway,  and  yaw  dynamics  and  neglects  heave,  roll,  and  pitch  dynamics.1 
The  vehicle-fixed  coordinate  system  is  positioned  at  the  center  of  mass  with  linear  translational 
velocities  {u,  v}  and  rotational  velocity  (r  =  9}.  Similarly,  the  interial  (earth-fixed)  reference 
frame  {X,Y}  is  positioned  at  the  center  of  mass  of  the  vehicle  with  linear  translational  velocities 
{. x,y }  and  rotational  velocity  {£?}. 


Figure  1.  Schematic  of  a  Generic  Surface  Vehicle 


The  rotation  transformation  matrix  (equation  (1))  used  to  map  between  vehicle-fixed  and  interial 
coordinates  is  given  by: 
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In  the  vehicle-fixed  reference  frame,  the  dynamics  of  the  vehicle  can  be  written  as  shown 
in  equation  (2): 

Mr)  +  C(j))r)  +  0(77)77  =/,  (2) 

where  r)  =  [uvr  ]Tis  a  state  vector,  M  is  the  added  mass  and  interial  matrix,  C  is  the  Coriolis 
and  centripetal  matrix.  D  is  the  damping  matrix,  and  /  are  the  control  forces  and  moments.  The 
mass  matrix  M  and  Coriolis  and  centripetal  matrix  C  can  be  further  decomposed  as  shown  in 
equation  (3) 


M  =  Mrb  +  Ma  and  C  =  CRB  +  CA 


(3) 
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The  mass  matrix  (equation  (4))  is  composed  of  rigid  body  contributions  (that  is,  mass  and 
inertia)  and  contributions  from  the  surrounding  fluid  (that  is,  “added  mass”).  The  mass  matrix 
can  be  written  as 


M  = 


m-Xu 

0 

0 


0  0 
m  —  Yy  mXg  —  Yt 
mxg  —  Yy.  l z  —  N-f. 


where  the  following  constants  are  used: 


m  =  153.94  kg 
/z  =  73.04  kg  m2 
Xu  =  —18.17  kg 
Y^  =  -124.54  kg 
Yf  =  0  kg  m 
Nf  =  —36.15  kg  m. 


(4) 


The  Coriolis  and  centripetal  matrix  is  written  as  shown  in  equation  (5): 


C(? l)  = 


0 

0 

— m(x5r  +  t?)  +  YyV  +  Y+r 


0  m(xgr  +  v)  —  (YvV  —  Tfr)' 

Yy  m  —  XyU 

(-Xy  +  m)u  0 


(5) 


The  damping  matrix  is  composed  of  linear  and  nonlinear  terms.  The  linear  damping  is 
due  to  laminar  friction  on  the  body  of  the  vehicle,  and  the  nonlinear  damping  results  from  vortex 
shedding  off  the  hull  of  the  vehicle.  The  composite  damping  matrix  is  written  as  shown  in 
equation  (6): 


\XU 
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o  - 

Xu\u\  |tt| 

0 

0  ' 

D  -  DL  +  DNL(rf)  - 
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where  the  following  constants  are  used: 

Xu  =  -107.33  kg/s 
Yv  =  -536.67  kg/s 
Yr  =  -322.00  kg  m/s 
Nv  =  —128.80  kg  m/s 
Nr  =  -1073.33  kg  m2/s 
*u|u|  =  -107.33  kg/m 
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Yv \v\  -  -536.67  kg/m 
Nr |r|  =  —322.00  kg  m2. 


The  control  force  and  moment  vector  is  written  as  shown  in  equation  (7): 


/  = 


'  P  cos  (p  ■ 
Psincp  , 
.a  P  sin  (p. 


(7) 


where  a  is  the  distance  from  the  bow  of  the  vehicle  to  the  center  of  mass  and  P  is  the  applied 
thrust  at  an  angle  (p. 


Next,  external  forcing  in  the  model  was  considered  in  the  form  of  a  constant  current  flow 

f  f 

(14  and  Vy  )  in  the  vehicle-fixed  reference  frame.  The  implementation  of  these  equations  can  be 
implemented  into  MATLAB  and  integrated  using  ODE45  (MATLABs  ordinary  differnential 
equation  solver).  The  equations  can  be  rewritten  in  the  inertial  frame  of  reference  as  a  set  of  six, 
first-order  ODEs  as  follows  (equation  (8))  where  (■)  represents  the  relative  difference  between 
the  velocity  component  and  the  current  flow  in  that  direction: 
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Vu  =  V/  cos  9  +  Vy  sin  9,  and 


Vv  =  —Vj  sin  6  +  Vy  cos  9  . 
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NUMERICAL  BENCHMARKING  OF  MODEL 


To  test  the  model  performance,  the  dynamic  response  due  to  several  initial  conditions 
was  investigated;  the  resulting  motion  trajectories  are  shown  in  figures  2  through  5.  In  each 
case,  the  vehicle  starts  at  [0,0].  For  the  case  depicted  in  figure  2,  the  current  flow  in  the 
horizontal  and  vertical  dimensions  are  v/  —  0.02  m/s  and  vj  =  0.01  m/s,  respectively.  The 
vehicle  is  propelled  by  Pu  =  P  cos  0  =  10  AT  thrust  in  the  u-direction,  and  Pv  =  P  sin  0  =  50  TV 
thrust  in  the  u-direction.  A  trajectory  length  of  only  2  minutes  is  shown  starting  from  a  position 
aligned  with  the  flow  and  zero-relative  velocity  with  respect  to  the  water.  In  figure  2,  plot  (A) 
shows  the  surface  trajectory  of  the  vehicle,  plot  (B)  shows  the  vehicle  displacements,  plot  (C) 
shows  velocity,  and  plot  (D)  shows  orientation  and  angular  rate  versus  time.  During  the 
2-minute  trajectory,  the  vehicle  rotated  at  a  constant  angular  rate  and  translated  in  both  X-  and 
Y-directions.  According  to  plot  (B),  the  vehicle  traversed  a  greater  distance  in  the  X-direction 
than  in  the  Y-direction — a  result  of  the  applied  thrust  in  the  Y-direction,  which  produced  a 
torque  on  the  vehicle  and  caused  the  vehicle  to  rotate  (plot  D).  Furthermore,  the  vehicle 
experienced  a  current  flow  that  was  twice  as  great  in  magnitude  in  the  X-direction  than  in  the 
Y-direction. 


Figure  2.  Sample  Motion  Trajectories  Generated  by  the  Vehicle 


for  v{  =  0.  02  m/s  and  Vy  =  0.  01  m/s 

(The  vehicle  is  propelled  by  Pu  =  10  N  thrust  in  the  u-direction  and  Pv  =  50  N  thrust  in  the 
v-direction.  Plot  (A)  shows  the  surface  trajectory  of  the  vehicle, 
plot  (B)  shows  the  vehicle  displacements,  plot  (C)  shows  velocity, 
and  plot  (D)  shows  orientation  and  angular  rate  versus  time.) 


5 


Figure  3  depicts  the  response  if  the  thrust  is  changed  to  Pu  =  1  N  and  Pv  =  50  N.  The 
same  dynamic  behavior  is  seen  in  this  sample  trajectory  as  in  the  first  case  depicted  in  figure  2; 
however,  with  such  a  decrease  of  applied  thrust  in  the  X-direction,  the  vehicle  rotated  more  in 
the  same  amount  of  time.  The  same  type  of  response  was  observed  in  the  first  case  (figure  2) 
except  there  were  more  rotations.  Again,  the  vehicle  rotated  at  a  constant  angular  velocity. 


Figure  3.  Sample  Motion  Trajectories  Generated  by  the  Vehicle 
for  V %  =  0.  02  m/s  and  Vy  =  0.  01  m/s 


(The  vehicle  is  propelled  by  Pu  =  IN  thrust  in  the  u-direction  and  Pv  =  50  N  thrust  in  the 
v-direction.  Plot  (A)  shows  the  surface  trajectory  of  the  vehicle, 
plot  (B)  shows  the  vehicle  displacements,  plot  (C)  shows  velocity, 
and  plot  (D)  shows  orientation  and  angular  rate  versus  time.) 


6 


Figure  4  depicts  the  effects  if  there  is  no  applied  thrust  in  either  direction  and  an  initial 
relative  linear  and  angular  velocity  of  v  =  1.7  m/s  and  r  =  10  rad/s,  respectively.  Without  an 
applied  thrust,  the  displacement  increased  linearly  and  the  velocity,  rotation,  and  angular  velocity 
all  remained  constant  after  the  transients  decay  due  to  the  initial  conditions.  The  steady-state 
response  was  reached  within  t  =  0.1  s.  The  large  jump  in  plot  (A)  between  the  first  and  second 
vehicle  plot  is  a  result  of  the  initial  condition.  At  the  first  time  step,  the  vehicle  is  located  at  [0,0] 
with  the  prescribed  initial  conditions.  At  the  second  time  step,  as  a  result  of  the  dynamics,  the 
velocity  decreased  to  almost  zero  and  rotation  became  constant;  however,  during  this  time  step,  a 
significant  change  in  Y-displacement  occurred. 


ID'I 


Time  r&fr 


Figure  4.  Sample  Motion  Trajectories  Generated  by  the  Vehicle  for  No  Applied  Thrust, 

=  0.  02  m/s  and  Vy  =  0. 01  m/s,  and  v  =  1. 7  m/s  Initial  Relative  Velocity 
and  r  =  10  rad/s  Angular  Velocity 

(Plot  (A)  shows  the  surface  trajectory  of  the  vehicle,  plot  (B)  shows  the  vehicle  displacements, 
plot  (C)  velocity,  and  plot  (D)  shows  orientation  and  angular  rate  versus  time.) 
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Figure  5  depicts  the  last  numerical  benchmark  of  the  vehicle  model.  The  same  conditions 
were  used  as  in  the  previous  case  (figure  4);  however,  the  current  flow  was  increased  to  vj  = 

0.2  m/s  and  Vy  =  0.2  m/s.  Again,  the  same  type  of  response  was  observed;  the  difference  is  the 
distance  that  was  traveled  in  both  the  X-  and  Y-directions  is  a  result  of  the  increased  current 
flow.  Again,  the  large  displacement  observed  in  plot  (A)  is  a  result  of  the  initial  condition. 


Figure  5.  Sample  Motion  Trajectories  Generated  by  the  Vehicle  for  No  Thrust  but 
with  v  =  1. 7  m/s  Initial  Relative  Velocity  and  r  =  10  rad/s  Angular  Velocity, 

v{.  —  0. 2  m/s  and  Vy  —  0. 2  m/s 

(Plot  (A)  shows  the  surface  trajectory >  of  the  vehicle,  plot  (B)  shows  the 
vehicle  displacements,  plot  (C)  shows  velocity,  and  plot  (D)  shows  orientation 

and  angular  rate  versus  time.) 
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TWO-POINT  BOUNDARY  VALUE  PROBLEM  AND  CONTINUATION 


The  overall  goal  of  this  investigation  is  to  determine  a  path  for  a  generic  surface  vehicle 
to  follow  that  passes  between  two  waypoints  in  a  predetermined  fixed  amount  of  time.  Without 
loss  of  generality,  only  two  waypoints  will  be  considered.  The  proposed  method  is  not 
concerned  if  these  waypoints  are  the  start  and  end  locations  of  a  specific  mission,  or  if  they  are 
interior  waypoints  in  a  larger  path-planning  exercise.  The  overarching  problem  of  determining  a 
trajectory  between  two  points  with  a  given  time  can  be  expressed  mathematically  as  a  two-point 
boundary  value  problem  (BVP).  The  standard  practice  lends  itself  to  solving  these  problems 
using  methods  such  as  finite  difference,  shooting,  and  collocation. 2,3  Other  techniques  to 
determine  optimal  paths  involve  heuristic  procedures  that  are  tuned  to  specific  application 
contexts4  and  are  thus  difficult  to  apply  when  a  specific  optimization  objective  is  required.  The 
two-point  BVP  described  in  the  following  paragraphs  uses  the  numerical  procedure  in 
MATLAB’s  BVP  (bvp4c)  algorithm  using  the  continuation  method. 

The  goal  is  to  find  a  solution  to  the  set  of  first-order  ODEs  that  were  previously  presented 
that  model  a  generic  surface  vehicle  on  the  time  interval  [0,  T ]  subject  to  two-point  boundary 
value  conditions: 

x(0)  -0  =  0 

u(0)  -  Pu  =  0 

y(o)  -o  =  o 

v(0)  -Vv-0  (9) 

r(0)  -0  =  0 
x(T)  -XT  =  0 
y(T )  -  yt  =  o 


where  the  initial  position  is  at  zero-time  from  zero-position  with  zero-relative  and  angular 
velocities,  and  the  final  position  is  ( XT ,  YT )  in  time  T.  In  addition  to  the  motion  trajectory,  the 
solution  to  the  BVP  should  also  provide  the  initial  orientation  0(0)  and  propulsion 
parameters (PUI  Pv ).  Generally,  there  are  multiple  solutions  to  this  particular  BVP,  but  the 
MATLAB’s  bvp4c  function  will  find  the  trajectory  and  thrust  parameters  based  on  an  initial 
guess  of  the  solution. 

Solving  the  BVP  problem  using  bvp4c  requires  an  initial  guess  for  the  solution.  Note 
that  the  algorithm  is  very  sensitive  to  the  particular  form  of  the  initial  guess.  The  quality  of  the 
initial  guess  is  paramount  in  convergence  speed  of  the  BVP  algorithm  and  often  determines  if  the 
solution  can  be  determined  practically.  To  facilitate  finding  a  good  initial  guess  that  will 
converge  to  the  solution,  if  available,  one  should  use  some  a  priori  information  about  the  system 
or  the  solution.  If  some  intermediate  solution  is  known,  one  can  use  continuation2,3  to  solve  the 
BVP  sequentially  by  incrementing  parameters  from  the  known  intermediate  solution  to  the 
needed  one — the  method  that  was  adopted  in  this  work  to  find  convergent  solutions. 
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NOMINAL  PATH  PLANNING  FOR  SURFACE  VEHICLES 


Before  determining  an  optimal  path  for  the  vehicle  to  follow  to  the  desired  end  location 
or  waypoint,  a  nominal  path  must  first  be  determined.  The  nominal  path  (NP)  is  defined  as  the 
path  that  requires  minimal  extended  propulsion  energy.  For  this  investigation,  a  constant,  slow 
surface  current  in  an  arbitrary  direction  rj  was  assumed.  There  is  an  intuitive  supposition  that 
sometimes  it  is  beneficial  (in  terms  of  expended  energy)  to  go  against  the  flow  for  a  short  period 
of  time  and  then  just  ride  the  current  to  the  target  without  any  propulsion.  In  particular,  here  we 
tested  a  hypothesis  that,  if  the  goal  is  achievable,  then  the  straight-line  path  to  the  target  is  the 
most  economical. 

The  schematic  of  a  test  configuration  is  shown  in  figure  6.  The  objective  is  to  reach  point 
B  ( XT ,  Yt )  in  a  fixed  time  T  starting  from  point  0  (0,0)  while  expending  minimal  energy.  Two 
strategies  for  reaching  the  goal  B  were  considered:  (1)  a  straight-line  motion  to  the  goal  (that  is 
from  0  to  B )  and  (2)  first  getting  to  the  current  flow  streamline  that  passes  through  the  goal  (that 
is,  from  0  to  A)  and  then  just  riding  that  streamline  to  the  goal  (that  is,  from  A  to  B)  with  no 
thrust.  Note  that  the  most  efficient  way  to  get  to  the  streamline  is  to  head  perpendicular  to  it 
(6  =  ?y2  +  vy 


Figure  6.  Geometry  for  Vehicle  NP  Planning 


In  practice,  there  would  be  some  energy  spent  reorienting  the  vehicle  at  point  A  once  the 
thrust  is  set  to  zero,  but  it  is  assumed  that  spent  energy  is  small  and  negligible.  Even  if  this 
energy  is  not  negligible,  it  will  only  add  to  the  cost  of  getting  to  the  streamline.  The  most 
economical  constant  amplitude  thrust  for  the  powered  straight-line  motions  from  0  to  B,  or  0 
to  A  was  used.  Time  spent  going  across  the  current  is  denoted  Tc  and  time  spent  gliding  along 
the  current  is  denoted  TF,  hence  the  overall  travel  time  is  T  =  Tc  +  TF. 

In  the  first  scenario  (strategy  1),  T  =  Tc,  then  the  average  velocity  required  to  reach  the 
goal  is  VB  =  dB/0/T ,  where  dB/0  is  a  distance  between  points  B  and  0.  In  addition,  if  the 
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corresponding  average  relative  velocity  of  the  vehicle  with  respect  to  the  water  is  greater  than  the 
maximum  possible  vehicle  velocity,  the  objective  is  unreachable.  To  calculate  the  actual  thrust 
required  for  reaching  the  goal,  the  two-point  BVP  using  the  vehicle  model  (equation  (8))  and  the 
following  initial  and  boundary  conditions  (equation  (10))  were  used: 


rXi 

U 

y 

V 

e 

-r- 

r-t 

II 

O 

0 

v/  cos  6  +  Vy  sin  9 
0 

—Vj  sin  9  +  Vy  cos  9 

do 

0 


(10) 


where  90  —  tan  1  — y.  After  determining  the  required  constant  thrust  P,  the  total  energy 

"x  — 

expended  by  the  vehicle  was  determined  by  integrating  the  product  of  Pu  with  u  (that  is,  the 
thrust  and  velocity  in  the  u  —direction,  respectively)  over  the  time  of  travel  T  (equation  (11)): 


E  =  fo  puudt  =  Ik=i  pkUkAt. 


(11) 


For  the  second  scenario  (strategy  2),  the  thrust  required  to  get  to  point  A  was  estimated 
using  a  continuation  from  the  solution  obtained  for  the  straight-line  motion  scenario  (first 
strategy).  In  this  case,  the  average  velocity  during  powered  travel  is  VA  =  dA/0/Tc.  In 
calculations,  the  time  spent  coasting  along  the  current  7>  was  incremented  from  0  to  Tp,  and  the 
corresponding  two-point  (for  the  time  from  0  to  Tc  =  T  —  TF)  BVP  was  solved  using  the 
solution  from  the  previous  iteration  step  and  the  boundary  conditions  shown  in  equation  (12): 


0 

rXi 

U 

Vj  cos  9  +  Vy  sin  9 

y 

0 

V 

—V/  sin  9  +  Vy  cos  9 

6 

-r- 

t= 0 

n/2  +  v 

0 

(12) 


where  Tp  is  a  critical  coasting  time  after  which  the  objective  is  not  reachable  even  using  the 
maximum  thrust  available. 


The  results  of  the  simulation  are  shown  in  figure  7,  where  the  calculated  expended  energy 

is  plotted  versus  the  ratio  of  y-.  The  resulting  curve  confirms  the  hypothesis  that  the  most 

Tc 

economical  path  to  the  goal  is  motion  on  a  straight  line  when  moving  with  constant  thrust.  A 
straight  line,  therefore,  was  chosen  as  an  NP  to  the  goal. 
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Figure  7.  Energy  Required  to  Reach  the  Goal  Versus  Ratio  of  Time  Spent 
Free-Floating  over  Time  Spent  on  Powered  Motion 


The  formulation  was  modified  to  look  at  constant  thrust  motion  from  0  to  B  with  the 
different  initial  orientation  of  the  vehicle  mimicking  the  previous  cases  (strategies  1  and  2).  In 
these  cases  there  were  no  energy  costs  associated  with  the  reorienting  at  point  A.  The  BVP  for 
each  case  was  solved  again  using  the  results  from  the  previous  iteration  while  the  angle  was 
iterated  from  the  initial  required  orientation  for  the  straight-line  motion  to  an  additional  90°.  The 
resultant  motion  trajectories  are  shown  in  the  left  plot  in  figure  8;  the  associated  energy 
expended  is  shown  in  the  right  plot  in  figure  8. 
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Figure  8.  Various  Achievable  Trajectories  with  Constant  Thrust  Propulsion  (Left  Plot) 
and  the  Corresponding  Energy  Scaling  with  the  Divergence  from  the 
Straight-Line  Path  (Right  Plot) 
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OPTIMAL  CONTROL  TO  FOLLOW  THE  NOMINAL  PATH 


It  has  been  established  that  an  NP  for  reaching  a  goal  in  fixed  time  is  a  straight-line  path. 
For  the  NP,  it  was  assumed  that  the  vehicle’s  initial  heading  was  parallel  to  the  optimal  heading 
and  its  initial  relative  velocity  was  zero.  In  practice,  both  assumptions  are  not  realizable,  and  the 
vehicle  could  have  any  arbitrary  initial  heading  and  velocities  within  an  allowable  range.  The 
vehicle  equations  have  demonstrated  that  the  vehicle  is  highly  maneuverable  (see  figures  2 
through  5)  and  can  reorient  itself  within  several  seconds  or  minutes.  Thus,  the  next  step  is  to 
focus  on  developing  an  optimal  control  strategy5,6  that  can  be  used  to  reorient  the  vehicle  and 
follow  the  NP.  During  the  simulations,  it  was  observed  that  the  controller’s  success  was  very 
sensitive  to  the  size  of  the  time  step  used  in  adjusting  the  control  input.  If  the  time  step  was  kept 
small,  the  controller  was  stable;  but  if  the  time  step  was  larger  than  the  time  it  took  to  reorient  the 
vehicle,  the  controller  failed.  In  practice,  one  may  want  to  split  the  control  strategy  into  two 
stages:  (1)  initial  reorientation  (during  which,  the  time  step  is  kept  small)  followed  by  (2)  NP 
tracking  (for  which,  the  time  step  can  be  increased  substantially). 


This  investigation  considered  the  vehicle  model  expressed  in  equation  (13)  for  optimal 
control  procedure: 


where 
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and 


Vu  =  Vj  cos  9  +  Vy  sin  9, 

Vv  —  —v/  sin  0  +  Vy  cos  9  . 

Upon  introduction  of  a  state  vector  z  =  [x,  u,  y,  v,  9,  r] T ,  equation  ( 1 3 )  is  rewritten  as : 
z  =  F(z,  P). 


(13) 
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The  corresponding  Jacobian  matrices  with  respect  to  state  and  control  variables  are 
represented  in  equation  (14): 
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where 


Al^  =  vj  sin  9  —  Vy  cos  9,  and  AVv  =  v/  cos  9  +  vj  sin  9  . 

Now,  let  tic,  which  is  uniformly  distributed  over  time,  be  the  times  at  which  possible 
adjustments  can  be  made  to  the  control  inputs.  These  time  steps  are  separated  by  h  —  At  —  T/N. 
Let  the  current  trajectory  be  z,  denoting  the  NP  z,  and  the  corresponding  constant  thrust  P.  Then 
the  deviation  from  the  NP  trajectory  is  defined  as: 


Az(tfc)  =  z(tk)  -  z(tfc). 


(15) 


This  deviation  leads  to  the  following  linear  discrete  approximation  to  the  equations  of  motion 
about  the  NP  trajectory  (equation  (16)): 


_  ,  .  dF 

zk+ 1  ~  zk  h  qz 


z=zk 


Azj  +  hgCP  +  AP,), 


(16) 


where  tk  =  kh,  zk  =  z(tfc).  Equation  (16)  is  rewritten  in  equation  (17): 


zk+ 1  —  Akzk  +  Bk(P  +  A  P k), 


(17) 


where 


Z=Zk 


and  Bk  =  h^- . 

K  dP 
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An  optimal  control  strategy  for  the  vehicle  was  found  based  on  equations  (18),  (19),  and 
(20)  by  using  dynamic  programming.  The  optimal  control  goal  was  to  minimize  the  following 
objective  function: 

F  =  a  zJ/Hzn  +  (1  -  a)  YlkZ l  zrkQkzk  +  APjPkAPk  (18) 

Using  standard  dynamic  programming  procedures,5  the  gain  used  for  control  was  calculated 
recursively  for  k  from  1  to  IV  —  1  using: 

G, N-k  ~  ~ [(1  fc  “b  B-n-kUk-iBn-k\  Bn-kU  k-\An_k  ,  (19) 


where 


Bk  —  ^N-k^k-l^N-k  +  (1  ~  a)6/V-/c^N-fc£/V-fc  +  (1  ~  a)Q/V-k» 


with 


UN  —  aH,  and  VN_k  =  AN_k  +  BN_kGk_1. 

The  appropriate  adjustment  to  the  thrust  at  the  k- th  time  step  was  then  calculated  as 


^Bk  Gk  (zk  zk). 


(20) 


The  application  of  this  thrust  adjustment  provided  an  optimal  control  that  followed  the  stated 
trajectory  as  best  as  possible,  given  the  constraints  of  the  vehicle  dynamics  and  available  control 
authority. 
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TESTING  THE  OPTIMAL  CONTROL  PROCEDURE 


The  effectiveness  of  the  optimal  control  procedure  was  demonstrated  by  testing  it  on  a 
problem  whose  initial  conditions  were  perturbations  from  the  initial  conditions  used  for 
determining  the  initial  optimal  path.  The  objective  was  to  start  in  the  vicinity  of  the  (0,0) 
position  and  reach  a  target  at  (X,  Y)  =  (1000,1000)  m  position  in  50  minutes.  We  assume  the 
current  is  0.2  m/s  in  x-direction  and  —0.2  m/s  in  the  y-direction.  The  NP  was  calculated  by 
solving  the  corresponding  BVP  (see  figure  9),  where  the  nominal  thrust  was  Pu  =  91.481  N. 


Figure  9.  NP  Used  in  Control  Testing 


The  initial  condition  for  NP  (■)  (equation  (21))  and  the  actual  initial  condition  given  to 
the  control  algorithm  (equation  (22))  are 
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rXi 

-  -1.3008  - 

U 

0.8824 

y 

10.7352 

V 

8.0658 

9 

-2.7238 

-r- 

t= 0 

0 

(22) 


For  the  objective  function,  we  set  a  =  0.9,  and  use  the  following  matrices: 
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and  Rk  =  5  ll-  (23) 


The  resulting  trajectories  are  shown  in  figure  10;  figure  1 1  shows  the  adjustments  needed 
in  the  control  inputs.  Figure  12  shows  a  blowup  of  the  resulting  trajectories  near  the  beginning 
of  motion,  and  figure  13  shows  the  difference  between  the  nominal  and  actual  trajectories. 


Figure  10.  Optimal  Control  Applied  to  the  Perturbed  Initial  Condition 
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Figure  11.  Adjustments  to  the  Applied  Thrust  Needed  for  Optimal  Control  in  Figure  10 


Figure  12.  Blow-Up  Near  the  Initial  Time  of  the  Optimal  Control  Shown  in  Figure  10 


Figure  13.  Difference  Between  the  Nominal  and  Actual  Trajectories 
for  the  Optimal  Control  Given  in  Figure  10 
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LIMITING  CONTROL  INPUT  MAGNITUDE  IN  OPTIMAL  CONTROL 


In  a  more  realistic  environment,  an  unlimited  amount  of  thrust  is  usually  not  available; 
therefore,  any  restrictions  that  meet  a  physical  behavior  or  desired  response  can  be  incorporated 
into  the  optimal  trajectory  determination  methodology.  Generally,  this  problem  is  solved  by 
incorporating  a  penalty  function  in  the  objective  function  or  implementing  a  constrained  optimal 
control  procedure  (using  interior  point  methods).  For  this  research,  the  clipping  procedure  was 
used;  that  is,  the  control  inputs  are  clipped  to  a  maximum  amplitude.  This  control  strategy  was 
tested  for  several  initial  conditions;  no  significant  degradation  in  controller  performance  was 
observed.  The  goal  NP  is  shown  in  figure  14,  where  the  objective  needs  to  be  reached  in  10 
seconds.  The  constant  thrust  estimated  by  solving  the  BVP  for  the  NP  was  91.628  N,  and  the 
maximum  possible  amplitude  was  set  to  be  125  N  (as  with  all  that  follow). 


Figure  14.  Optimal  Constrained  Control  Applied  to  the  Perturbed  Initial  Condition 


Figure  15  shows  the  constrained  controlled  tracking  of  the  NP;  the  corresponding  clipped 
control  inputs  are  shown  in  figure  1 6.  The  deviations  between  the  nominal  and  actual 
trajectories  are  given  in  figure  1 7.  The  initial  condition  for  the  NP  (equation  (24))  and  the  actual 
initial  condition  given  to  the  control  algorithm  (equation  (25))  are 
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For  the  objective  function,  we  set  a  =  0.95,  and  all  other  matrices  were  the  same. 


Figure  15.  Optimal  Constrained  Control  Applied  to  the  Perturbed  Initial  Condition 


Figure  16.  Applied  Thrust  Variations  Needed  for  Control  Shown  in  Figure  15 
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Figure  1 7. 
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A  x  (m)  -> 

Difference  Between  the  Nominal  and  Actual  Trajectories 
for  the  Optimal  Control  Given  in  Figure  15 


Figure  15  shows  that  it  is  clear  that  10  s  is  not  enough  time  to  achieve  the  objective  in 
this  situation;  therefore,  simulations  for  a  larger  time  and  space  for  the  exact  same  objective  and 
NP  shown  in  figure  9  were  run.  The  results  are  shown  in  figures  18,  19,  and  20  for  when  the 
initial  condition  for  NP  (equation  (26))  and  the  actual  initial  conditions  given  to  the  control 
algorithm  (equation  (27))  are 
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It  is  clear  from  these  extended-time  trajectories  that  the  allowance  of  additional  time 
creates  a  more  gradual  deviation  using  moderate  levels  of  thrust  over  the  extended  time.  Also, 
the  additional  time  allows  for  a  controlled  trajectory  that  does  reach  the  goal  point. 
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Figure  18.  Optimal  Constrained  Control  Applied  to  the  Perturbed  Initial  Condition 


Figure  19.  Applied  Thrust  Variations  Needed  for  Control  Shown  in  Figure  18 


Figure  20.  Difference  Between  the  Nominal  and  Actual  Trajectories  for 
the  Optimal  Control  Given  in  Figure  18 
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SUMMARY  AND  CONCLUSIONS 


The  concept  of  optimal  path  planning  and  optimal  control  of  a  generic  surface  vehicle  in 
a  constant  current  flow  was  investigated.  A  three  degree-of-freedom,  rigid-body  model  for  a 
surface  vehicle  was  developed,  its  parameters  were  estimated,  and  its  performance  was  tested  in 
simulations.  The  resulting  model  was  used  to  estimate  a  nominal  path  of  motion  for  a  vehicle  by 
solving  a  two-point  boundary  value  problem.  It  was  shown  in  a  relatively  static  current  flow  that 
a  straight-line  constant  trust  motion  was  optimal  in  terms  of  energy  consumption.  An  optimal 
control  algorithm  was  developed  to  follow  the  determined  nominal  path  of  motion  and  was 
extensively  tested  in  various  conditions.  For  sufficiently  small  time  steps,  the  optimal  control 
strategy  was  able  to  follow  the  nominal  trajectory  even  when  substantial  perturbations  were 
introduced  in  the  initial  conditions.  The  algorithm  was  also  modified  to  limit  range-of-control 
input  values  and  was  still  capable  of  following  pre-planned  trajectory. 

In  more  dynamic  or  nonlinear  current  flows,  the  solution  of  a  two-point  boundary  value 
problem  becomes  more  problematic  and  straight-line  paths  are  no  longer  optimal.  It  is  still 
feasible  that  the  constant  thrust  motion  will  be  the  most  advantageous  even  in  these  dynamic, 
nonlinear  conditions.  It  may  be  possible  to  solve  the  two-point  boundary  value  problem  in  a 
static  nonlinear  current  flow  environment,  but  some  other  path  planning  methodology  is  required 
for  a  dynamic  flow/obstacle  environment.  Metaheuristic  search  methods  to  find  the  energy- 
efficient  paths  is  one  such  possibility  and  is,  in  fact,  the  planned  methodology  for  the  next  phase 
of  this  research. 
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